%NEEDLESHAPEGENERATOR Simulates needle steering to generate curves
%
% author:   Troy Adebar
% date:     01/06/2014

clear all; close all; clc;
%% Define constants for the simulation ------------------------------------
% Number of points
N = 200;
% Steering parameters
l = 0.5;                              % insertion increment (mm)
r = 50;                               % minimum radius of curvature (mm)
% Kalman filter constants
n = 6;                                % size of the state vector
m = 6;                                % size of the measurement vector
% Initial needle pose
x(:,:,1) = [0 0 0 0 0 0]';   % [x y z (mm) a b g (rad)]
% Process noise mean and variance
mu_w = zeros(n,1);                    
Q = diag([0.1,0.1,0.6,0.001,0.001,0.001]); % measured variance

%JOEY: MULTIPLY Q BY A FACTOR IF YOU WANT TO CHANGE VARIATIONS IN CURVE
% EG:
% Q = Q*0; %<---- Gives ideal needle steering model
% Q = Q*0.00001;
% Q = Q*0.001;
% Q = Q*0.1;
 Q = Q*1; %<---- Approximates experimentally measured deviation from model

%% Run simulation loop ----------------------------------------------------
display(N)
d_th = 0;
for t = 1:N
    % Define control inputs for the current time
    if(mod(t, 50) == 0) 
        d_th = randi(10, 1);
    else 
        d_th = 0;
    end
    %d_th = 0;% JOEY: Add logic here if you want to generate random curves
    u(:,1,t) = [ d_th r l ]';
    
    % Simulate process noise
    w = (Q)^.5*randn(n,1)+mu_w;
    % Propogate the actual state forward (simulation)
    x(:,:,t+1) = f(x(:,:,t),u(:,:,t),w);
end

%% Plot actual needle pose generated by simulation
% Format the plot
hfig = figure(1); clf;
axis equal;
grid on;
xlabel('x-axis (mm)'); ylabel('z-axis'); zlabel('y-axis');
set(gca,'ZDir','reverse');
view(-180,90);
set(gcf,'Color',[1 1 1]);
title('Needle Steering Simulation - Actual State');
% Highlight the entry position and target
hold on;
plot3(x(1,:,1),x(3,:,1),x(2,:,1),'ko','MarkerSize',10,...
    'MarkerFaceColor','k');
hold off;
% Plot the state vectors
for t = 1:size(x,3)
    plotstate(x(:,1,t),hfig,2);
end

%eof